### Project 4 Gesture Detection
By mounting an MPU6050 on the glove we can read the Roll and Pitch angles in real time and output the corresponding gestures through the serial port, paving the way for later Bluetooth transmission or mecanum-car control.
**1.Attitude-Angle Overview**
With only an MPU6050 on the glove, the Yaw (heading) angle drifts, so we read only Roll and Pitch.
For complete three-axis angles, add a magnetometer for calibration.
**Roll (Y-axis)**
Fingers pointing forward; rotate the hand left/right around the palm.
| Angle | Gesture |
| ------ | ------------------ |
| 0 ° | Hand level |
| –180 ° | Fully rolled left |
| +180 ° | Fully rolled right |
**Pitch (X-axis)**
Fingers pointing forward; raise/lower the hand around the palm.
| Angle | Gesture |
| ------ | ----------------- |
| 0 ° | Hand level |
| –180 ° | Hand raised |
| +180 ° | Hand pressed down |
**2.Example Gesture Ranges**
When the angles fall into the ranges below, you can treat them as the corresponding gestures (feel free to adjust):
1. Level: X and Y both –10 ° ~ 10 °
2. Roll-left: Y axis –20 ° ~ –90 °
3. Roll-right: Y axis 20 ° ~ 90 °
4. Raise-up: X axis 20 ° ~ 90 °
5. Press-down: X axis –20 ° ~ –90 °
Keep Bluetooth disconnected while uploading code; after flashing, open the Serial Monitor to view real-time angles and gesture text.
**3.Code**
```c
#include "MPU6050.h"
MPU6050lib mpu;
float aRes, gRes; // scale resolutions per LSB for the sensors
int16_t accelCount[3]; // Stores the 16-bit signed accelerometer sensor output
int16_t gyroCount[3]; // Stores the 16-bit signed gyro sensor output
float ax, ay, az; // Stores the real accel value in g's
float gyrox, gyroy, gyroz; // Stores the real gyro value in degrees per seconds
float gyroBias[3] = {0, 0, 0};
float accelBias[3] = {0, 0, 0}; // Bias corrections for gyro and accelerometer
int16_t tempCount; // Stores the real internal chip temperature in degrees Celsius
float temperature;
float SelfTest[6];
float q[4] = {1.0f, 0.0f, 0.0f, 0.0f};// vector to hold quaternion
float pitch, yaw, roll;
// parameters for 6 DoF sensor fusion calculations
float GyroMeasError = PI * (40.0f / 180.0f); //gyroscope measurement error in rads/s (start at 60 deg/s), then reduce after ~10 s to 3
float beta = sqrt(3.0f / 4.0f) * GyroMeasError; // compute beta(β)
float GyroMeasDrift = PI * (2.0f / 180.0f); // gyroscope measurement drift in rad/s/s (start at 0.0 deg/s/s)
float zeta = sqrt(3.0f / 4.0f) * GyroMeasDrift; // compute zeta, the other free parameter in the Madgwick scheme usually set to a small or zero value
float deltat = 0.0f; // integration interval for both filter schemes
uint32_t lastUpdate = 0, firstUpdate = 0; // used to calculate integration interval
uint32_t Now = 0; // used to calculate integration interval
void setup()
{
Wire.begin();
Serial.begin(9600);
// Read the WHO_AM_I register, this is a good test of communication
uint8_t c = mpu.readByte(MPU6050_ADDRESS, WHO_AM_I_MPU6050); // Read WHO_AM_I register for MPU-6050
Serial.print("I AM ");
Serial.println(c, HEX);
mpu.settings(AFS_8G, GFS_250DPS);
if (c == 0x68) //WHO_AM_I should always be 0x68
{
Serial.println("MPU6050 is online...");
// Start by performing self test and reporting values
mpu.MPU6050SelfTest(SelfTest);
Serial.print("x-axis self test: acceleration trim within : "); Serial.print(SelfTest[0],1); Serial.println("% of factory value");
Serial.print("y-axis self test: acceleration trim within : "); Serial.print(SelfTest[1],1); Serial.println("% of factory value");
Serial.print("z-axis self test: acceleration trim within : "); Serial.print(SelfTest[2],1); Serial.println("% of factory value");
Serial.print("x-axis self test: gyration trim within : "); Serial.print(SelfTest[3],1); Serial.println("% of factory value");
Serial.print("y-axis self test: gyration trim within : "); Serial.print(SelfTest[4],1); Serial.println("% of factory value");
Serial.print("z-axis self test: gyration trim within : "); Serial.print(SelfTest[5],1); Serial.println("% of factory value");
if (SelfTest[0] < 1.0f && SelfTest[1] < 1.0f && SelfTest[2] < 1.0f && SelfTest[3] < 1.0f && SelfTest[4] < 1.0f && SelfTest[5] < 1.0f) {
Serial.println("Pass Selftest!");
// Calibrate gyro and accelerometers, load biases in bias registers
mpu.calibrateMPU6050(gyroBias, accelBias);
Serial.println("MPU6050 bias");
Serial.println(" x\t y\t z ");
Serial.print((int)(1000 * accelBias[0])); Serial.print('\t');
Serial.print((int)(1000 * accelBias[1])); Serial.print('\t');
Serial.print((int)(1000 * accelBias[2]));
Serial.println(" mg");
Serial.print(gyroBias[0], 1); Serial.print('\t');
Serial.print(gyroBias[1], 1); Serial.print('\t');
Serial.print(gyroBias[2], 1);
Serial.println(" o/s");
mpu.settings(AFS_2G, GFS_250DPS);
mpu.initMPU6050();
// Initialize device for active mode read of accelerometer , gyroscope, and temperature
Serial.println("MPU6050 initialized for active data mode....");
}
}
else
{
Serial.print("Could not connect to MPU6050: 0x");
Serial.println(c, HEX);
while(1); // Loop forever if communication doesn't happen
}
}
void loop()
{
// If data ready bit set, all data registers have new data
if (mpu.readByte(MPU6050_ADDRESS, INT_STATUS) & 0x01) { // check if data ready interrupt
mpu.readAccelData(accelCount); // Read the x/y/z adc values
aRes = mpu.getAres(); // Acquire the converted value
// Now we'll calculate the accleration value into actual g's
ax = (float)accelCount[0] * aRes; // get actual g value, this depends on scale being set
ay = (float)accelCount[1] * aRes;
az = (float)accelCount[2] * aRes;
mpu.readGyroData(gyroCount); // Read the x/y/z adc values
gRes = mpu.getGres(); // Acquire the converted value
// Calculate the gyro value into actual degrees per second
gyrox = (float)gyroCount[0] * gRes; // get actual gyro value, this depends on scale being set
gyroy = (float)gyroCount[1] * gRes;
gyroz = (float)gyroCount[2] * gRes;
tempCount = mpu.readTempData(); // Read the x/y/z adc values
temperature = ((float) tempCount) / 340. + 36.53; // Temperature in degrees Centigrade
}
// Acquire the current time of the system in ms
Now = micros();
// set integration time by time elapsed since last filter update
deltat = ((Now - lastUpdate) / 1000000.0f);
lastUpdate = Now;
if(lastUpdate - firstUpdate > 10000000uL) {
beta = 0.041; // decrease filter gain after stabilized
zeta = 0.015; // increase gyro bias drift gain after stabilized
}
// Convert the gyroscope data to radians
gyrox = gyrox * PI / 180.0f;
gyroy = gyroy * PI / 180.0f;
gyroz = gyroz * PI / 180.0f;
// Quaternion conversion function
MadgwickQuaternionUpdate(ax, ay, az, gyrox, gyroy, gyroz);
// Define output variables from updated quaternion---these are Tait-Bryan angles, commonly used in aircraft orientation.
// In this coordinate system, the positive z-axis is down toward Earth.
// Yaw is the angle between Sensor x-axis and Earth magnetic North (or true North if corrected for local declination, looking down on the sensor positive yaw is counterclockwise.
// Pitch is angle between sensor x-axis and Earth ground plane, toward the Earth is positive, up toward the sky is negative.
// Roll is angle between sensor y-axis and Earth ground plane, y-axis up is positive roll.
// These arise from the definition of the homogeneous rotation matrix constructed from quaternions.
// Tait-Bryan angles as well as Euler angles are non-commutative; that is, the get the correct orientation the rotations must be
// applied in the correct order which for this configuration is yaw, pitch, and then roll.
yaw = atan2(2.0f * (q[1] * q[2] + q[0] * q[3]), q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3]);
pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2]));
roll = atan2(2.0f * (q[0] * q[1] + q[2] * q[3]), q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]);
pitch *= 180.0f / PI;
yaw *= 180.0f / PI;
roll *= 180.0f / PI;
if(40 >= pitch && pitch >= -40
&& 40 >= roll && roll >= -40){
Serial.println("Gestures:Horizontal");
}
else if (-40 >= pitch && pitch >= -90
&& 40 >= roll && roll >= -40){
Serial.println("Gestures:Hand to the left");
}
else if (90 >= pitch && pitch >= 40
&& 40 >= roll && roll >= -40){
Serial.println("Gestures:Hand to the right");
}
else if (-40 >= roll && roll >= -90
&& 40 >= pitch && pitch >= -40){
Serial.println("Gestures:Hand down");
}
else if (90 >= roll && roll >= 20
&& 40 >= pitch && pitch >= -40){
Serial.println("Gestures:Hand up");
}
delay(100);
}
// Implementation of Sebastian Madgwick's "...efficient orientation filter for... inertial/magnetic sensor arrays"
// which fuses acceleration and rotation rate to produce a quaternion-based estimate of relative
// device orientation -- which can be converted to yaw, pitch, and roll. Useful for stabilizing quadcopters, etc.
// The performance of the orientation filter is at least as good as conventional Kalman-based filtering algorithms
// but is much less computationally intensive---it can be performed on a 3.3 V Pro Mini operating at 8 MHz!
void MadgwickQuaternionUpdate(float ax, float ay, float az, float gyrox, float gyroy, float gyroz)
{
float q1 = q[0], q2 = q[1], q3 = q[2], q4 = q[3]; // short name local variable for readability
float norm; //vector norm
float f1, f2, f3; // objective funcyion elements
float J_11or24, J_12or23, J_13or22, J_14or21, J_32, J_33; // objective function Jacobian
float qDot1, qDot2, qDot3, qDot4;
float hatDot1, hatDot2, hatDot3, hatDot4;
float gerrx, gerry, gerrz, gbiasx, gbiasy, gbiasz; // gyro bias error
// Auxiliary variables to avoid repeated arithmetic
float _halfq1 = 0.5f * q1;
float _halfq2 = 0.5f * q2;
float _halfq3 = 0.5f * q3;
float _halfq4 = 0.5f * q4;
float _2q1 = 2.0f * q1;
float _2q2 = 2.0f * q2;
float _2q3 = 2.0f * q3;
float _2q4 = 2.0f * q4;
float _2q1q3 = 2.0f * q1 * q3;
float _2q3q4 = 2.0f * q3 * q4;
// Normalise accelerometer measurement
norm = sqrt(ax * ax + ay * ay + az * az);
if (norm == 0.0f) return; // handle NaN
norm = 1.0f/norm;
ax *= norm;
ay *= norm;
az *= norm;
// Compute the objective function and Jacobian
f1 = _2q2 * q4 - _2q1 * q3 - ax;
f2 = _2q1 * q2 + _2q3 * q4 - ay;
f3 = 1.0f - _2q2 * q2 - _2q3 * q3 - az;
J_11or24 = _2q3;
J_12or23 = _2q4;
J_13or22 = _2q1;
J_14or21 = _2q2;
J_32 = 2.0f * J_14or21;
J_33 = 2.0f * J_11or24;
// Compute the gradient (matrix multiplication)
hatDot1 = J_14or21 * f2 - J_11or24 * f1;
hatDot2 = J_12or23 * f1 + J_13or22 * f2 - J_32 * f3;
hatDot3 = J_12or23 * f2 - J_33 *f3 - J_13or22 * f1;
hatDot4 = J_14or21 * f1 + J_11or24 * f2;
// Normalize the gradient
norm = sqrt(hatDot1 * hatDot1 + hatDot2 * hatDot2 + hatDot3 * hatDot3 + hatDot4 * hatDot4);
hatDot1 /= norm;
hatDot2 /= norm;
hatDot3 /= norm;
hatDot4 /= norm;
// Compute estimated gyroscope biases
gerrx = _2q1 * hatDot2 - _2q2 * hatDot1 - _2q3 * hatDot4 + _2q4 * hatDot3;
gerry = _2q1 * hatDot3 + _2q2 * hatDot4 - _2q3 * hatDot1 - _2q4 * hatDot2;
gerrz = _2q1 * hatDot4 - _2q2 * hatDot3 + _2q3 * hatDot2 - _2q4 * hatDot1;
// Compute and remove gyroscope biases
gbiasx += gerrx * deltat * zeta;
gbiasy += gerry * deltat * zeta;
gbiasz += gerrz * deltat * zeta;
gyrox -= gbiasx;
gyroy -= gbiasy;
gyroz -= gbiasz;
// Compute the quaternion derivative
qDot1 = -_halfq2 * gyrox - _halfq3 * gyroy - _halfq4 * gyroz;
qDot2 = _halfq1 * gyrox + _halfq3 * gyroz - _halfq4 * gyroy;
qDot3 = _halfq1 * gyroy - _halfq2 * gyroz + _halfq4 * gyrox;
qDot4 = _halfq1 * gyroz + _halfq2 * gyroy - _halfq3 * gyrox;
// Compute then integrate estimated quaternion derivative
q1 += (qDot1 -(beta * hatDot1)) * deltat;
q2 += (qDot2 -(beta * hatDot2)) * deltat;
q3 += (qDot3 -(beta * hatDot3)) * deltat;
q4 += (qDot4 -(beta * hatDot4)) * deltat;
// Normalize the quaternion
norm = sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4); // 标准化四元数 normalise quaternion
norm = 1.0f/norm;
q[0] = q1 * norm;
q[1] = q2 * norm;
q[2] = q3 * norm;
q[3] = q4 * norm;
}
```
**Gestures:Horizontal**
- **Meaning:** The sensor is lying flat (level with the ground).
- **Trigger:** Both Pitch and Roll are small (between -40° and 40°).
**Gestures:Hand to the left**
- **Meaning:** You have tilted the sensor to the **Left**.
- **Trigger:** Pitch is between -40° and -90°.
**Gestures:Hand to the right**
- **Meaning:** You have tilted the sensor to the **Right**.
- **Trigger:** Pitch is between 40° and 90°.
**Gestures:Hand down**
- **Meaning:** You have tilted the sensor **Downwards** (pointing down).
- **Trigger:** Roll is between -40° and -90°.
**Gestures:Hand up**
- **Meaning:** You have tilted the sensor **Upwards** (pointing up).
- **Trigger:** Roll is between 20° and 90°.